#include "FishThread.h"
#include "quadfish.h"

int FishThread::NumThreads = 0;
QuadTree *FishThread::Root = NULL;

FishThread::FishThread():
        // n_fishOwned(0),
        maxNorm( 0 )
{
    movedFish.resize( NumThreads );
    for ( int i = 0;i < NumThreads;i++ ) {
        movedFish[i] = new fishAndNodeVector;
    }
}

FishThread::~FishThread()
{
    for ( int i = 0;i < NumThreads;i++ ) {
        delete movedFish[i];
    }
}

void FishThread::addLeaf( LeafNode* node )
{
    leafNodes.insert( node );
}

void FishThread::removeLeaf( LeafNode *node )
{
    leafNodes.erase( node );
}

void FishThread::addRoot( TreeNode *node )
{
    rootNodes.insert( node );
}

void FishThread::removeRoot( TreeNode *node )
{
    rootNodes.erase( node );
}

void FishThread::cleanupRoots()
{
    TreeSet::iterator tmptsi;
    for ( TreeSet::iterator i = rootNodes.begin();
            i != rootNodes.end();
            i = tmptsi ) {
        tmptsi = i;
        tmptsi++;
        ( *i )->cleanupRoots( *this );
    }
}

void FishThread::clearRootsLeaves()
{
    rootNodes.clear();
    leafNodes.clear();
}

void FishThread::interactFish()
{
    for ( LeafSet::iterator i = leafNodes.begin();i != leafNodes.end();i++ ) {
        ( *i )->intraQuadInteract();
        ( *i )->interQuadInteract();
    }
}

void FishThread::computeNorm()
{
    double max = 0;
    for ( LeafSet::iterator i = leafNodes.begin();i != leafNodes.end();i++ ) {
        LeafNode &l = **i;
        for ( FishList::iterator j = l.contents.begin(); j != l.contents.end();j++ ) {
            fish_t &fish = **j;
            double m0 = f_max( fabs( fish.vy ), fabs( fish.vx ) );
            double m1 = f_max( fabs( fish.ax ) , fabs( fish.ay ) );
            double m2 = f_max( m0, max );
            max = f_max( m1, m2 );
        }
    }
    maxNorm = max;
}

void FishThread::moveFish( double dt, int myID )
{
    for ( LeafSet::iterator i = leafNodes.begin();i != leafNodes.end();i++ ) {
        LeafNode &l = **i;
        int counter = 0;
        for ( FishList::iterator j = l.contents.begin();
                j != l.contents.end();
                counter++ ) {
            fish_t &fish = **j;

            double v4x, v4y, v5x = 0, v5y = 0;
            double r, r2;
            // calculate random unit vector
            v4x = ( drand48() - 0.5 );
            v4y = ( drand48() - 0.5 );
            r2 = f_max( v4x * v4x + v4y * v4y, min_r2 );
            r = sqrt( r2 );
            v4x /= r;
            v4y /= r;

            if ( fish.in_sight != 0 ) {
                // calculate vector pointing in the direction of the average velocity of nearby flockmates
                fish.v2x /= fish.in_sight;
                fish.v2y /= fish.in_sight;

                fish.v2x = fish.v2x - fish.vx;
                fish.v2y = fish.v2y - fish.vy;

                fish.cogx /= fish.in_sight;
                fish.cogy /= fish.in_sight;

                // calculate vector towards cog of local flockmates
                v5x = fish.cogx - fish.x;
                v5y = fish.cogy - fish.y;

                fish.numInteractions++;
            }

            if ( fish.in_crowd != 0 ) {
                fish.v0x /= fish.in_crowd;
                fish.v0y /= fish.in_crowd;

                double r2 = fish.v0x * fish.v0x + fish.v0y * fish.v0y;

                // let the repulsion decrease with r2?
                fish.v0x /= r2 * r2;
                fish.v0y /= r2 * r2;
            }

            // now we have v0 to v5...
            double vx = C0 * fish.v0x + C2 * fish.v2x + C4 * v4x + C5 * v5x;
            double vy = C0 * fish.v0y + C2 * fish.v2y + C4 * v4y + C5 * v5y;

            fish.ax = M * vx;
            fish.ay = M * vy;

            fish.x += dt * fish.vx;
            fish.y += dt * fish.vy;
            fish.vx += dt * fish.ax;
            fish.vy += dt * fish.ay;

            bounceFish( fish );
            //if fish no longer belongs here, find its new home
            if ( !l.belongsHere( fish ) ) {
                int newThread = -1;
                fishAndNode tmp;
                tmp.fish = *j;
                tmp.node = NULL;
                j = l.contents.erase( j );

                Root->findHome( *tmp.fish, tmp.node, newThread );
                /*
                 * if fish is moving to another of my LeafNodes, just insert it right now
                 * else it's moving to anothe thread's LeafNodes, so queue it for
                 * later insertion
                 */
                if ( newThread == myID ) {
                    tmp.node->insert( tmp.fish );
                } else {
                    movedFish[newThread]->push_back( tmp );
                }
            } else {
                j++;
            }
        }
    }
}

void FishThread::bounceFish( fish_t &fish )
{
    while ( fish.x < LEFT_WALL || fish.x > RIGHT_WALL ) {
        if ( fish.x < LEFT_WALL ) {
            fish.x += WALL_SEP;
        }
        if ( fish.x > RIGHT_WALL ) {
            fish.x -= WALL_SEP;
        }
    }
    while ( fish.y < LEFT_WALL || fish.y > RIGHT_WALL ) {
        if ( fish.y < LEFT_WALL ) {
            fish.y += WALL_SEP;
        }
        if ( fish.y > RIGHT_WALL ) {
            fish.y -= WALL_SEP;
        }
    }
}

void FishThread::transferFish( int otherThread )
{
    while ( !movedFish[otherThread]->empty() ) {
        fishAndNode &tmp = movedFish[otherThread]->back();
        tmp.node->insert( tmp.fish );
        movedFish[otherThread]->pop_back();
    }
}

void FishThread::balance()
{
    nFishOwned = 0;

    TreeSet::iterator tmptsi;
    for ( TreeSet::iterator i = rootNodes.begin();
            i != rootNodes.end();
            i = tmptsi ) {
        //collapse may invalidate the iterator
        tmptsi = i;
        tmptsi++;
        nFishOwned += ( **i ).updateFishCount();
        ( **i ).collapse( *this );
    }
    LeafSet::iterator tmplsi;
    for ( LeafSet::iterator i = leafNodes.begin();
            i != leafNodes.end();
            i = tmplsi ) {
        /*
         * if leafnode has no parent, or its parent is not owned (is not someone's root node)
         * then we need to count this node's contents
         */
        if (( **i ).parent == NULL || ( **i ).parent->owner == -1 ) {
            nFishOwned += ( **i ).contents.size();
        }
        //we do this because splitUp may invalidate the iterator
        tmplsi = i;
        tmplsi++;
        ( **i ).splitUp( *this );
    }
}

void FishThread::outputFish( FILE* output_fp, const double t, const double dt, int &count )
{
    cg_x = 0.0;
    cg_y = 0.0;

    for ( LeafSet::iterator i = leafNodes.begin();i != leafNodes.end();i++ ) {
        for ( FishList::iterator j = ( *i )->contents.begin();
                j != ( *i )->contents.end();
                j++ ) {
            cg_x += ( *j )->x;
            cg_y += ( *j )->y;
            fprintf( output_fp, "  %d: (%g, %g)\n", count++,
                     scale_coord(( *j )->x ), scale_coord(( *j )->y ) );
        }
    }
}

double FishThread::averageInteractions()
{
    double fishInteractedWith = 0, numInteractions = 0;
    for ( LeafSet::iterator i = leafNodes.begin();i != leafNodes.end();i++ ) {
        for ( FishList::iterator j = ( *i )->contents.begin();
                j != ( *i )->contents.end();
                j++ ) {
            fishInteractedWith += ( *j )->fishInteractedWith;
            numInteractions += ( *j )->numInteractions;
        }
    }
    return fishInteractedWith / numInteractions;
}
